/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "IMUFileReader.hpp"
#include "owndebug.h"
int main(void){
  CIMUFileReader reader;
  // Init Debug lib
  debugInit();
  debugSetGlobalDebugLvl(1);

  reader.ReadFile("Avant635.MaCI_IMU.3DM-GX1_21_12_2010_10:41:55.txt");

  std::list<gim::time> times;
  if(reader.GetTimes(times)){
    dPrint(1,"Got times... number of times %d",times.size());
  }else{
    dPrint(1,"Failed to get times ");
    exit(1);
  }

 dPrint(1,"***************************************************************************************");
  TIMUDataMap map;
  if(reader.GetAllData(map)){
    dPrint(1,"Got all data. Size of the map = %d",map.size());
  }else{
    dPrint(1,"Failed to get all the data");
    exit(1);
  }
 dPrint(1,"***************************************************************************************");
  MaCI::IMU::CIMUData data;
  std::list<gim::time>::iterator iterFirst= times.begin();
  if(reader.GetIMUData(data,*iterFirst)){
    dPrint(1,"Got first IMU data..");
    data.Print(1);
  }else{
    dPrint(1,"Failed to get the first data");
    exit(1);
  }
 dPrint(1,"***************************************************************************************");
  std::list<gim::time>::iterator iterLast = times.end();
  iterLast--;
  if(reader.GetIMUData(data,*iterLast)){
    dPrint(1,"Got last IMU data..");
    data.Print(1);
  }else{
    dPrint(1,"Failed to get the last data at %u %u",iterLast->seconds,iterLast->useconds)
    exit(1);
  }

 dPrint(1,"***************************************************************************************");
  dPrint(1,"Getting last acceleration value");
  MaCI::IMU::TAcceleration3d acceleration;
  if(reader.GetAcceleration3d(acceleration,*iterLast)){

    dPrint(1,"got last acceleration value (%f, %f, %f)",
           acceleration.accelerationX,
           acceleration.accelerationY,
           acceleration.accelerationZ);
  }else{
    dPrint(1,"Failed to get the last acceleration data at %u %u",iterLast->seconds,iterLast->useconds)
  
  }
  dPrint(1,"***************************************************************************************");
  
  dPrint(1,"Getting last angular velocity value");
  MaCI::IMU::TAngularVelocity3d angular;
  if(reader.GetAngularVelocity3d(angular,*iterLast)){
    
    dPrint(1,"got last angular velocity value (%f, %f, %f)",
           angular.wX,
           angular.wY,
           angular.wZ);
  }else{
    dPrint(1,"Failed to get the last acceleration data at %u %u",iterLast->seconds,iterLast->useconds);
      
  }
  dPrint(1,"***************************************************************************************");

  dPrint(1,"Getting last angular velocity value");
  MaCI::IMU::TAngle3d angle;
  if(reader.GetAngle3d(angle,*iterLast)){
    
    dPrint(1,"got last angle value (%f, %f, %f)",
           angle.roll,
           angle.pitch,
           angle.yaw);
  }else{
    dPrint(1,"Failed to get the last  angular velocity data at %u %u",iterLast->seconds,iterLast->useconds);
      
  }
  dPrint(1,"***************************************************************************************");
  dPrint(1,"Getting last quaternion value");
  MaCI::IMU::TQuaternion quat;
  if(reader.GetQuaternion(quat,*iterLast)){
    
    dPrint(1,"got last quaternion (%f, %f, %f,%f)",
           quat.scalar,
           quat.x,
           quat.y,
           quat.z);
  }else{
    dPrint(1,"Failed to get the last  quaternion data at %u %u",iterLast->seconds,iterLast->useconds);
      
  }
  dPrint(1,"***************************************************************************************");

  /*


  bool GetOrientationMatrix(const MaCI::IMU::TOrientationMatrix *aOrientationMatrix,
                            gim::time aTime);


  */
  MaCI::IMU::TOrientationMatrix ori;
  dPrint(1,"Getting last orientation matrix value");
  if(reader.GetOrientationMatrix(ori,*iterLast)){
    
    dPrint(1,"got last orientation matrix (%f, %f, %f); (%f, %f, %f); (%f, %f, %f)",
           ori.e11,
           ori.e12,
           ori.e13,
           ori.e21,
           ori.e22,
           ori.e23,
           ori.e31,
           ori.e32,
           ori.e33)
  }else{
    dPrint(1,"Failed to get the last  angular velocity data at %u %u",iterLast->seconds,iterLast->useconds);
      
  }
  dPrint(1,"***************************************************************************************");


  dPrint(1,"***************************************************************************************");
  dPrint(1,"***************************************************************************************");
  dPrint(1,"***************************************************************************************");
  dPrint(1,"***************************************************************************************");
  dPrint(1,"Test ended succesfully. Congratulations!!! :) ");
  dPrint(1,"***************************************************************************************");
  dPrint(1,"***************************************************************************************");
  dPrint(1,"***************************************************************************************");

}










